#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <math.h>
#include "kalman.h"


float getRand(float min, float max) {
	return min + (((float)rand()/(float)RAND_MAX) * (max - min));
}

int main(void) {
	
	
	// input and state information
	kal_t P[36]       // 6x6 error covariance
	= {0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
		0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
		0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
		0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
		0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
		0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
	kal_t K[36];      // 6x6 kalman gain
	kal_t x[6] 		  // state information {acceleration_x, acceleration_y, 
	// velocity_x, velocity_y, position_x, position_y}
	= {0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
	kal_t y[6];       // sensor input {acceleration_x, acceleration_y, 
	// velocity_x, velocity_y, position_x, position_y}
	kal_t u[2];       // commanded input {acceleration_x, acceleration_y}
	
	
	
	
	int i;
	unsigned long reps = 500;
	
	float orientation = 0.0;
	float acceleration = 0.0;
	float d = 0.0;
	float accx0, accy0;
	float diffx = 0.0 ,diffy = 0.0;
	float velx0, vely0, velx1, vely1;
	velx0 = 0.0, vely0 = 0.0;
	float posx0, posy0;
	posx0 = 0.0, posy0 = 0.0;
	
	int tw = 0;
	for (; tw < 1; ++tw) {
		// stabalization period...
		int decelerate = 0;
		int stop = 0;
		for (i = 0; i < reps; ++i) {
			
			if (!decelerate && d >= 1.0) {
				decelerate = 1;
				d = 1.0;
			}
			if (decelerate && !stop && acceleration <= 0.0) {
				stop = 1;
				acceleration = 0.0;
			}
			
			if (!decelerate) 
				d += .01;
			else if (!stop)
				d -= .01;
			else 
				d = 0.0;
			
			acceleration += d;
			
			float ycomponent = sin(orientation);
			float xcomponent = cos(orientation);
			
			diffx = accx0;
			diffy = accy0;
			accx0 = xcomponent*acceleration;
			accy0 = ycomponent*acceleration;
			diffx = accx0 - diffx;
			diffy = accy0 - diffy;
			
			velx1 = accx0 + velx0;
			vely1 = accy0 + vely0;
			
			posx0 = .5*accx0 + velx1 + posx0;
			posy0 = .5*accy0 + vely1 + posy0;
			
			y[0] = accx0+getRand(-0.5,0.5);
			y[1] = 0;//accy0+getRand(-0.5,0.5);
			y[2] = velx1+getRand(-1.0,1.0);
			y[3] = 0;//vely1+getRand(-1.0,1.0);
			y[4] = posx0+getRand(-1.5,1.5);
			y[5] = 0;//posy0+getRand(-1.5,1.5);
			
			//printf("%f %f %f %f %f %f ",y[0],y[1],y[2],y[3],y[4],y[5]);
			
			u[0] = 0;//diffx;//accx0;
			u[1] = 0;//diffy;//accy0;
			kalman(P,K,y,x,u);
			
			//printf("%f %f %f %f %f %f ",accx0,accy0,velx1,vely1,posx0,posy0);
			//printf("%f %f %f %f %f %f\n",x[0],x[1],x[2],x[3],x[4],x[5]);
			
			velx0 = velx1;
			vely0 = vely1;
		}
	}	
}

